#!/usr/bin/env python3
# coding=utf-8

import rospy
from std_msgs.msg import String


def NavResultCallback(msg):
    rospy.logwarn("导航 = %s",msg.data)

if __name__ == "__main__":

    rospy.init_node("wp_node") #初始化节点

    navi_pub = rospy.Publisher("/waterplus/navi_waypoint", String, queue_size=10)  #发布话题
    res_sub = rospy.Subscriber("/waterplus/navi_result",String,NavResultCallback,queue_size=10) #订阅话题

    rospy.sleep(1) #停止时间1s

    navi_msg = String()
    navi_msg.data = "1"
    navi_pub.publish(navi_pub)

    rospy.spin()